#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File         :formation_structure.py
@Description  :
@Time         :2024/05/21 09:38:44
@Author       :Lin Yuheng
@Version      :1.0
'''


import numpy as np
import math

from sympy import false


# # 固定翼动力学模型参数
class S_FwModelParams:
    def __init__(self):
        ### tecs参数 ###

        # 最小油门
        self.throttle_min = 0.3

        # 最大油门
        self.throttle_max = 1.0

        # 巡航油门
        self.cruise_throttle = 0.5

        # 最小俯仰角rad
        self.pitch_min_rad = (
            - math.pi / 18 # -pi/4
        )  # 设置较小的失速度迎角 10度  PI/36  18  根据QGC的门限设为45度

        # 最大俯仰角rad
        self.pitch_max_rad = math.pi / 18 # pi/4

        # 最大俯仰角速度rad/s
        self.pitch_rate_max = math.pi / 3

        ### later_controller参数 ###

        # 最大滚转角rad
        self.roll_max_rad = math.pi * 40 / 180 # 40度

        # 最大滚转角速度rad/s
        self.roll_rate_max = math.pi / 3

        ### 空速控制器参数 ###

        # 飞机前向最大瞬时加速度
        self.maxinc_acc = 20.0

        # 飞机减速最大瞬时加速度
        self.maxdec_acc = 10.0

        # 飞机空速最大设定值，此处的最大速度，一定要和飞机的最快速度贴合，否则容易造成油门抖动
        self.max_airspeed_sp = 25.0

        # 飞机空速最小设定值
        self.min_airspeed_sp = 12.0  # QGC采取的模型中STALL_SPEED = 12.0

# 领机从机相对状态量
class S_FwRelStates:
    def __init__(self):
        pass

# 领机状态量
class S_FwLeaderStates:
    def __init__(self):
        # 姿态信息
        self.pitch_angle = 0
        self.yaw_angle = 0
        self.roll_angle = 0

        # 有时候并不能直接得到领机的航向信息
        self.yaw_valid = False

        # NED位置
        self.ned_pos_n = 0
        self.ned_pos_e = 0
        self.ned_pos_d = 0

        # NED速度和GPS速度不一致
        self.ned_vel_n = 0
        self.ned_vel_e = 0
        self.ned_vel_d = 0

        # GPS速度
        self.global_vel_n = 0
        self.global_vel_e = 0
        self.global_vel_d = 0
        self.latitude = 0
        self.longitude = 0
        self.altitude = 0
        self.relative_alt = 0
        self.air_speed = 0
        self.wind_estimate_n = 0
        self.wind_estimate_e = 0
        self.wind_estimate_d = 0

        # 角速度，从imu获得
        self.roll_rate = 0
        self.pitch_rate = 0
        self.yaw_rate = 0

# 本机状态信息
class S_FwStates:
    def __init__(self):
        # 飞机模式
        self.flight_mode = None

        # 姿态信息
        self.pitch_angle = 0
        self.yaw_angle = 0
        self.roll_angle = 0
        self.att_quat = np.zeros(4)

        # NED位置
        self.ned_pos_n = 0
        self.ned_pos_e = 0
        self.ned_pos_d = 0

        # NED速度
        self.ned_vel_n = 0
        self.ned_vel_e = 0
        self.ned_vel_d = 0

        # GPS速度
        self.global_vel_n = 0
        self.global_vel_e = 0
        self.global_vel_d = 0
        self.body_acc = np.zeros(3)

        # 角速度,从imu获得
        self.roll_rate = 0
        self.pitch_rate = 0
        self.yaw_rate = 0

        # 内部计算后赋值，或从外部赋值
        self.ned_acc = np.zeros(3)
        self.rotmat = np.zeros([3,3])
        self.latitude = 0
        self.longitude = 0
        self.altitude = 0
        self.relative_alt = 0
        self.air_speed = 0
        self.ground_speed = 0
        self.wind_estimate_x = 0
        self.wind_estimate_y = 0
        self.wind_estimate_z = 0
        self.in_air = True
        self.altitude_lock = false
        #TODO:添加yaw_valid的判断
        self.yaw_valid = True

# 编队队形配置
class S_FwFormationOffset:
    def __init__(self):
        # 机体系
        self.xb = 0
        self.yb = 0
        self.zb = 0

        # 航迹系
        self.xk = 0
        self.yk = 0
        self.zk = 0

        # NED系
        self.ned_n = 0
        self.ned_e = 0
        self.ned_d = 0

# 固定翼角度以及油门的期望值，由运动学位置以及速度的期望值以及当前飞机的状态计算
class S_FwSetPoint:
    def __init__(self):

        self.ned_pos_n = 0
        self.ned_pos_e = 0
        self.ned_pos_d = 0

        self.ned_vel_n = 0
        self.ned_vel_e = 0
        self.ned_vel_d = 0

        self.latitude = 0
        self.longtitude = 0
        self.altitude = 0
        self.relative_alt = 0

        self.air_speed = 0
        self.ground_speed = 0
        
        #期望加速度和滚转角
        self.a_t = 0
        self.phi = 0
        
# 本机误差
class S_FwError:
    def __init__(self):
        # ned坐标系下的位置误差
        self.P_N = 0
        self.P_E = 0
        self.P_D = 0
        self.P_NE = 0

        # 机体系位置误差<与自己期望>
        self.PXb = 0
        self.PYb = 0
        self.PZb = 0

        # 航迹系位置误差<与自己期望>
        self.PXk = 0
        self.PYk = 0
        self.PZk = 0

        # 机体系速度误差<与自己期望>
        self.VXb = 0
        self.VYb = 0
        self.VZb = 0

        # 航迹系速度误差<与自己期望>
        self.VXk = 0
        self.VYk = 0
        self.VZk = 0

        # 机体系速度误差<与领机>
        self.led_fol_vxb = 0
        self.led_fol_vyb = 0
        self.led_fol_vzb = 0
        self.led_fol_vb = 0

        # 航迹系速度误差<与领机>
        self.led_fol_vxk = 0
        self.led_fol_vyk = 0
        self.led_fol_vzk = 0
        self.led_fol_vk = 0

        # 航迹系下速度角度（方向）误差
        # 航向角误差
        self.led_fol_eta = 0
        
        # 编队误差
        self.ux = 0
        self.uy = 0
        self.pos_error_x = 0
        self.pos_error_y = 0
        self.vel_error_x = 0
        self.vel_error_y = 0
        self.pos_error_sum_x = 0
        self.pos_error_sum_y = 0
        self.vel_error_sum_x = 0
        self.vel_error_sum_y = 0

# 四通道控制量
class S_FW4Cmd:
    def __init__(self):
        self.throttle = 0
        self.pitch = 0
        self.roll = 0
        self.yaw = 0 # 这里默认yaw为0 

# 控制器X方向混合误差产生参数,控制器参数
class S_MixXerrParams:
    def __init__(self):
        # 速度比例项
        self.kv_p = 0.4
        # 位置误差比例项
        self.kp_p = 0.65
        # 总混合产生期望空速PID参数
        self.mix_kp = 0.3
        self.mix_kd = 0.00001
        self.mix_ki = 0.00008

# 控制器Y方向混合误差产生参数,控制器参数
class S_MixYerrParams:
    def __init__(self):
        # 领从机位置差比例项
        self.kp_p = 0.05
        # 领机从机速度方向
        self.keta_p = 2
        # 总混合产生期望roll PID参数
        self.mix_kp = 0.3
        self.mix_kd = 0.005
        self.mix_ki = 0.0015

class S_FormationMethod:
    def __init__(self):
        #TODO 添加编队控制器分段
        pass